#include "Camera.hpp"
#include "siut/simd/Vec3f.hpp"

using namespace siut;

#define PIOVER180 0.0174532925199f

Camera::Camera()
{
  //horizAng = 0.0f;
  //vertiAng = 45.0f * PIOVER180;

  // Hardcoded camera view and position
  horizAng = 5.5f;
  vertiAng = 0.80f;
  //pos = siut::simd::Vec4f(0.0,-50.0,-50,1);
  pos = siut::simd::Vec4f(250.0,-250.0,-250,1);


  dir = siut::simd::Vec4f(0.0);

}

Camera::~Camera()
{

}

void Camera::privateInit()
{
}


void Camera::privateRender()
{
  // not drawing any camera geometry
	
}

void Camera::privateUpdate()
{
	move();
}

void Camera::moveRight(float s)
{
  dir[0] = -s;
}
void Camera::moveLeft(float s)
{
  dir[0] = s;
}
void Camera::moveUp(float s)
{
  dir[1] = s;
}
void Camera::moveDown(float s)
{
  dir[1] = -s;
}
void Camera::moveForward(float s)
{
  dir[2] = s;
}
void Camera::moveBackward(float s)
{
  dir[2] = -s;
}

void Camera::setRot(float h, float v)
{
  horizAng += h;
  vertiAng += v;
}
void Camera::getRot(float &h, float &v)
{
  h = horizAng;
  v = vertiAng;
}
void Camera::move()
{
  // 1. Let siut do the math:
  //siut::simd::Mat4f xrotation, yrotation;
  //setIdentity(xrotation); setIdentity(yrotation);
  //rotateX(xrotation,vertiAng); rotateY(yrotation,horizAng);
  //siut::simd::Mat4f rotmat = xrotation * yrotation;

  //pos += dir * rotmat; // Move camera pos with direction vector and rotation
  //dir = siut::simd::Vec4f(0.0); // set direction vector to zero.

  //matrix_ = rotmat;
  //translate(matrix_, pos[0],  pos[1],  pos[2]);

  // 2. Eller saa kan vi gjore matta selv... vel deler av den.
  // Make the combined x,y-rotation matrix ourselves:
  siut::simd::Vec3f xrot( cos(horizAng), 0, -sin(horizAng) );
  siut::simd::Vec3f zrot( cos(vertiAng) * sin(horizAng), sin(vertiAng), cos(vertiAng)*cos(horizAng) );
  siut::simd::Vec3f yrot = zrot.cross( xrot ); 
  siut::simd::Mat4f rotmat( xrot[0], yrot[0], zrot[0], 0,
  							xrot[1], yrot[1], zrot[1], 0,
  							xrot[2], yrot[2], zrot[2], 0,
  							0,0,0, 1);

  pos += dir * rotmat; // Move camera pos with direction vector and rotation
  dir = siut::simd::Vec4f(0.0); // set direction vector to zero.
	
  // Make translation matrix
  siut::simd::Mat4f tx(
	  1, 0, 0, 0,
	  0, 1, 0, 0,
	  0, 0, 1, 0,
	  pos[0],  pos[1],  pos[2], 1
  );
  matrix_ = rotmat * tx; // V = OT, that is, View = Orientation * Translation

}

